﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace CPF_experement
{
    class WorldState : IComparable<WorldState>
    {
        int time_G;
        public int totalTime_G;
        public int huristic_H;
        public AgentState[] allAgentsState;
        int[][] moves;
        public WorldState prevStep;

        public WorldState(AgentState[] allAgentsState)
        {
            this.allAgentsState = allAgentsState;
            time_G = 0;
            setHuristic();
            totalTime_G = 0;
            moves = new int[allAgentsState.Length][];
            for (int i = 0; i < moves.Length; i++)
            {
                moves[i] = new int[4];
            }
        }
        public WorldState(WorldState cpy)
        {
            this.time_G = cpy.time_G;
            this.huristic_H = cpy.huristic_H;
            this.allAgentsState = new AgentState[cpy.allAgentsState.Length];
            for (int i = 0; i < allAgentsState.Length; i++)
            {
                this.allAgentsState[i] = new AgentState(cpy.allAgentsState[i]);
            }
            moves = new int[allAgentsState.Length][];
            for (int i = 0; i < moves.Length; i++)
            {
                moves[i] = new int[4];
            }
            for (int i = 0; i < moves.Length; i++)
            {
                for (int j = 0; j < 4; j++)
                {
                    this.moves[i][j] = cpy.moves[i][j];
                }
            }
        }
        public Boolean goalTest()
        {
            return huristic_H == 0;
        }

        public int CompareTo(WorldState that)
        {
            if (this.huristic_H + this.totalTime_G < that.huristic_H + that.totalTime_G)
                return -1;
            if (this.huristic_H + this.totalTime_G > that.huristic_H + that.totalTime_G)
                return 1;
            if (this.totalTime_G < that.totalTime_G)
                return 1;
            if (this.totalTime_G > that.totalTime_G)
                return -1;
            return 0;
        }
        private void setHuristic()
        {
            huristic_H = 0;
            foreach (AgentState temp in allAgentsState)
            {
                huristic_H += temp.huristic;
            }
        }
        private void setTotalTime_G()
        {
            totalTime_G = 0;
            foreach (AgentState temp in allAgentsState)
            {
                if (temp.huristic == 0)
                    totalTime_G += temp.arrivalTime;
                else
                    totalTime_G += time_G;
            }
        }
        private bool isValidMove(int from_X, int from_Y, int to_X,int to_Y, int agentNum)
        {
            if (True_Path_Huristic.isValidTile(to_X, to_Y))
            {
                for (int i = 0; i < agentNum; i++)
                {
                    if ((moves[i][2]==to_X && moves[i][3]==to_Y) || (moves[i][0] == to_X && moves[i][1] == to_Y && moves[i][2] == from_X && moves[i][3] == from_Y))
                        return false;
                }
                return true;
            }
            return false;
        }
        public void expandNode(int agentNum)
        {
            WorldState prev = this.prevStep;
            if (agentNum == 0) // If this is the first agent that moves
            {
                prev = this;
            }
            if (agentNum==allAgentsState.Length) // If all the agents have moved
            {
                //if (this.allAgentsState[0].pos_X == 0 && this.allAgentsState[0].pos_Y == 2 && this.allAgentsState[1].pos_X == 2 && this.allAgentsState[1].pos_Y == 1 && this.allAgentsState[2].pos_X == 0 && this.allAgentsState[2].pos_Y == 1 && this.allAgentsState[3].pos_X == 1 && this.allAgentsState[3].pos_Y == 1 && this.allAgentsState[4].pos_X == 1 && this.allAgentsState[4].pos_Y == 2 && this.allAgentsState[5].pos_X == 2 && this.allAgentsState[5].pos_Y == 2 && this.allAgentsState[6].pos_X == 1 && this.allAgentsState[6].pos_Y == 0 && this.allAgentsState[7].pos_X == 0 && this.allAgentsState[7].pos_Y == 0)
                //{
                //    bool t = true;
                //}
                setHuristic();
                time_G++;
                setTotalTime_G();
                if (!A_star.closedList.Contains(this))
                {
                    A_star.openList.Add(this);
                    A_star.closedList.Add(this);
                    A_star.generated++;
                }
                return;
            }
            int pos_X = allAgentsState[agentNum].pos_X;
            int pos_Y = allAgentsState[agentNum].pos_Y;
            moves[agentNum][0] = pos_X;
            moves[agentNum][1] = pos_Y;

            if (isValidMove(pos_X, pos_Y, pos_X, pos_Y , agentNum))
            {
                moves[agentNum][2] = pos_X;
                moves[agentNum][3] = pos_Y;
                WorldState temp = new WorldState(this);
                temp.prevStep = prev;
                temp.expandNode(agentNum + 1);
            }
            if (isValidMove(pos_X, pos_Y, pos_X-1,pos_Y,agentNum))
            {
                moves[agentNum][2] = pos_X - 1;
                moves[agentNum][3] = pos_Y;
                WorldState temp = new WorldState(this);
                temp.allAgentsState[agentNum].move(0,time_G+1);
                temp.prevStep = prev;
                temp.expandNode(agentNum + 1);
            }
            if (isValidMove(pos_X, pos_Y, pos_X, pos_Y + 1,agentNum))
            {
                moves[agentNum][2] = pos_X;
                moves[agentNum][3] = pos_Y+1;
                WorldState temp = new WorldState(this);
                temp.allAgentsState[agentNum].move(1, time_G+1);
                temp.prevStep = prev;
                temp.expandNode(agentNum + 1);
            }
            if (isValidMove(pos_X, pos_Y, pos_X + 1, pos_Y, agentNum))
            {
                moves[agentNum][2] = pos_X + 1;
                moves[agentNum][3] = pos_Y;
                WorldState temp = new WorldState(this);
                temp.allAgentsState[agentNum].move(2, time_G+1);
                temp.prevStep = prev;
                temp.expandNode( agentNum + 1);
            }
            if (isValidMove(pos_X, pos_Y, pos_X, pos_Y - 1, agentNum))
            {
                moves[agentNum][2] = pos_X;
                moves[agentNum][3] = pos_Y-1;
                WorldState temp = new WorldState(this);
                temp.allAgentsState[agentNum].move(3, time_G+1);
                temp.prevStep = prev;
                temp.expandNode( agentNum + 1);
            }
            if (A_star.diagonal)
            {
                if (isValidMove(pos_X, pos_Y, pos_X - 1, pos_Y + 1,agentNum))
                {
                    moves[agentNum][2] = pos_X - 1;
                    moves[agentNum][3] = pos_Y+1;
                    WorldState temp = new WorldState(this);
                    temp.allAgentsState[agentNum].move(4, time_G+1);
                    temp.prevStep = prev;
                    temp.expandNode( agentNum + 1);
                }
                if (isValidMove(pos_X, pos_Y, pos_X + 1, pos_Y + 1,  agentNum))
                {
                    moves[agentNum][2] = pos_X+1;
                    moves[agentNum][3] = pos_Y + 1;
                    WorldState temp = new WorldState(this);
                    temp.allAgentsState[agentNum].move(5, time_G+1);
                    temp.prevStep = prev;
                    temp.expandNode( agentNum + 1);
                }
                if (isValidMove(pos_X, pos_Y, pos_X + 1, pos_Y - 1, agentNum))
                {
                    moves[agentNum][2] = pos_X + 1;
                    moves[agentNum][3] = pos_Y-1;
                    WorldState temp = new WorldState(this);
                    temp.allAgentsState[agentNum].move(6, time_G+1);
                    temp.prevStep = prev;
                    temp.expandNode( agentNum + 1);
                }
                if (isValidMove(pos_X, pos_Y, pos_X, pos_Y - 1,  agentNum))
                {
                    moves[agentNum][2] = pos_X-1;
                    moves[agentNum][3] = pos_Y - 1;
                    WorldState temp = new WorldState(this);
                    temp.allAgentsState[agentNum].move(7, time_G+1);
                    temp.prevStep = prev;
                    temp.expandNode( agentNum + 1);
                }
            }
        }

        public override string ToString()
        {
            string ans = "step num: "+time_G+" huristic: "+huristic_H+" total time: "+totalTime_G+"\n";
            foreach (AgentState temp in allAgentsState)
            {
                ans +=" agent" + temp.agent.agentNum + ": (" + temp.pos_X + "," + temp.pos_Y + ")\n";
            }
            return ans;
        }
        public override int GetHashCode()
        {
            int ans = 0;
            int[] prime = {1,2,3,5,7,11,13,17,19,23,29,31,37,41,43,47,53,59,61,71,73,79};
            for (int i = 0; i < allAgentsState.Length; i++)
            {
                ans += allAgentsState[i].pos_X * prime[i % 20];
                ans += allAgentsState[i].pos_Y * prime[(i + 10) % 20];
            }
            return ans;
        }

        public override bool Equals(object obj)
        {
            WorldState that = (WorldState)obj;
            for (int i = 0; i < allAgentsState.Length; i++)
            {
                if (!this.allAgentsState[i].Equals(that.allAgentsState[i]))
                    return false;
            }
            if (this.huristic_H + this.totalTime_G > that.huristic_H + that.totalTime_G)
            {
                return false;
            }
            return true;
        }
    }
    class Travor_WorldState : IComparable<Travor_WorldState>
    {
        int time_G;
        public int totalTime_G;
        int huristic_H;
        public AgentState[] allAgentsState;
        int[][] moves;
        public int agentTurn;
        public Travor_WorldState prevStep;

        public Travor_WorldState(AgentState[] allAgentsState)
        {
            this.allAgentsState = allAgentsState;
            time_G = 0;
            setHuristic();
            totalTime_G = 0;
            agentTurn = 0;
            moves = new int[allAgentsState.Length][];
            for (int i = 0; i < moves.Length; i++)
            {
                moves[i] = new int[4];
            }
        }
        public Travor_WorldState(Travor_WorldState cpy)
        {
            this.time_G = cpy.time_G;
            this.huristic_H = cpy.huristic_H;
            this.allAgentsState = new AgentState[cpy.allAgentsState.Length];
            for (int i = 0; i < allAgentsState.Length; i++)
            {
                this.allAgentsState[i] = new AgentState(cpy.allAgentsState[i]);
            }
            this.agentTurn = (cpy.agentTurn + 1) % allAgentsState.Length;
            moves = new int[allAgentsState.Length][];
            for (int i = 0; i < moves.Length; i++)
            {
                moves[i] = new int[4];
            }
            for (int i = 0; i < moves.Length; i++)
            {
                for (int j = 0; j < 4; j++)
                {
                    this.moves[i][j] = cpy.moves[i][j];
                }
            }
            this.prevStep = cpy.prevStep;
            if (agentTurn==1 || allAgentsState.Length==1)
            {
                this.prevStep = cpy;
            }
            if (agentTurn == 0)
            {
                time_G++;
            }

        }
        public Boolean goalTest()
        {
            return huristic_H == 0;
        }

        public int CompareTo(Travor_WorldState that)
        {
           if (this.huristic_H + this.totalTime_G < that.huristic_H + that.totalTime_G)
                return -1;
            if (this.huristic_H + this.totalTime_G > that.huristic_H + that.totalTime_G)
                return 1;
            if (this.totalTime_G < that.totalTime_G)
                return 1;
            if (this.totalTime_G > that.totalTime_G)
                return -1;
            return 0;
        }
        private void setHuristic()
        {
            huristic_H = 0;
            foreach (AgentState temp in allAgentsState)
            {
                huristic_H += temp.huristic;
            }
        }
        private void setTotalTime_G()
        {
            totalTime_G = 0;
            for(int i=0 ;i<allAgentsState.Length ;i++)
            {
                if (allAgentsState[i].huristic == 0)
                    totalTime_G += allAgentsState[i].arrivalTime;
                else
                {
                    totalTime_G += time_G;
                    if (i < agentTurn)
                        totalTime_G++;
                }
            }
        }
        private bool isValidMove(int from_X, int from_Y, int to_X,int to_Y, int agentNum)
        {
            if (True_Path_Huristic.isValidTile(to_X, to_Y))
            {
                for (int i = 0; i < agentNum; i++)
                {
                    if ((moves[i][2]==to_X && moves[i][3]==to_Y) || moves[i][0] == to_X && moves[i][1] == to_Y && moves[i][2] == from_X && moves[i][3] == from_Y)
                        return false;
                }
                return true;
            }
            return false;
        }
        public void expandNode()
        {
            int pos_X = allAgentsState[agentTurn].pos_X;
            int pos_Y = allAgentsState[agentTurn].pos_Y;
            moves[agentTurn][0] = pos_X;
            moves[agentTurn][1] = pos_Y;

            if (isValidMove(pos_X, pos_Y, pos_X, pos_Y, agentTurn))
            {
                moves[agentTurn][2] = pos_X;
                moves[agentTurn][3] = pos_Y;
                Travor_WorldState temp = new Travor_WorldState(this);
                temp.setTotalTime_G();
                if (!Trevor_A_star.closedList.Contains(temp))
                {
                    Trevor_A_star.openList.Add(temp);
                    Trevor_A_star.closedList.Add(temp);
                    Trevor.generated++;
                }
            }
            if (isValidMove(pos_X, pos_Y, pos_X - 1, pos_Y, agentTurn))
            {
                moves[agentTurn][2] = pos_X - 1;
                moves[agentTurn][3] = pos_Y;
                Travor_WorldState temp = new Travor_WorldState(this);
                temp.allAgentsState[agentTurn].move(0,time_G+1);
                temp.allAgentsState[agentTurn].direction = 1;
                temp.setTotalTime_G();
                if (!Trevor_A_star.closedList.Contains(temp))
                {
                    temp.setHuristic();
                    Trevor_A_star.openList.Add(temp);
                    Trevor_A_star.closedList.Add(temp);
                    Trevor.generated++;
                }
            }
            if (isValidMove(pos_X, pos_Y, pos_X, pos_Y + 1, agentTurn))
            {
                moves[agentTurn][2] = pos_X;
                moves[agentTurn][3] = pos_Y + 1;
                Travor_WorldState temp = new Travor_WorldState(this);
                temp.allAgentsState[agentTurn].move(1, time_G + 1);
                temp.allAgentsState[agentTurn].direction = 2;
                temp.setTotalTime_G();
                if (!Trevor_A_star.closedList.Contains(temp))
                {
                    temp.setHuristic();
                    Trevor_A_star.openList.Add(temp);
                    Trevor_A_star.closedList.Add(temp);
                    Trevor.generated++;
                }
            }
            if (isValidMove(pos_X, pos_Y, pos_X + 1, pos_Y, agentTurn))
            {
                moves[agentTurn][2] = pos_X + 1;
                moves[agentTurn][3] = pos_Y;
                Travor_WorldState temp = new Travor_WorldState(this);
                temp.allAgentsState[agentTurn].move(2, time_G + 1);
                temp.allAgentsState[agentTurn].direction = 3;
                temp.setTotalTime_G();
                if (!Trevor_A_star.closedList.Contains(temp))
                {
                    temp.setHuristic();
                    Trevor_A_star.openList.Add(temp);
                    Trevor_A_star.closedList.Add(temp);
                    Trevor.generated++;
                }
            }
            if (isValidMove(pos_X, pos_Y, pos_X, pos_Y - 1, agentTurn))
            {
                moves[agentTurn][2] = pos_X;
                moves[agentTurn][3] = pos_Y - 1;
                Travor_WorldState temp = new Travor_WorldState(this);
                temp.allAgentsState[agentTurn].move(3, time_G + 1);
                temp.allAgentsState[agentTurn].direction = 4;
                temp.setTotalTime_G();
                if (!Trevor_A_star.closedList.Contains(temp))
                {
                    temp.setHuristic();
                    Trevor_A_star.openList.Add(temp);
                    Trevor_A_star.closedList.Add(temp);
                    Trevor.generated++;
                }
            }
            //if (A_star.diagonal)
            //{
            //    if (isValidMove(pos_X, pos_Y, pos_X - 1, pos_Y + 1, agentTurn))
            //    {
            //        moves[agentTurn][2] = pos_X - 1;
            //        moves[agentTurn][3] = pos_Y + 1;
            //        Travor_WorldState temp = new Travor_WorldState(this);
            //        temp.allAgentsState[agentTurn].move(4, time_G + 1);
            //        temp.allAgentsState[agentTurn].direction = 5;
            //        if (!Trevor_A_star.closedList.Contains(temp))
            //        {
            //            temp.setHuristic();
            //            temp.setTotalTime_G();
            //            Trevor_A_star.openList.Add(temp);
            //            Trevor_A_star.closedList.Add(temp);
            //            Trevor.generated++;
            //        }
            //    }
            //    if (isValidMove(pos_X, pos_Y, pos_X + 1, pos_Y + 1, agentTurn))
            //    {
            //        moves[agentTurn][2] = pos_X + 1;
            //        moves[agentTurn][3] = pos_Y + 1;
            //        Travor_WorldState temp = new Travor_WorldState(this);
            //        temp.allAgentsState[agentTurn].move(5, time_G + 1);
            //        temp.allAgentsState[agentTurn].direction = 6;
            //        if (!Trevor_A_star.closedList.Contains(temp))
            //        {
            //            temp.setHuristic();
            //            temp.setTotalTime_G();
            //            Trevor_A_star.openList.Add(temp);
            //            Trevor_A_star.closedList.Add(temp);
            //            Trevor.generated++;
            //        }
            //    }
            //    if (isValidMove(pos_X, pos_Y, pos_X + 1, pos_Y - 1, agentTurn))
            //    {
            //        moves[agentTurn][2] = pos_X + 1;
            //        moves[agentTurn][3] = pos_Y - 1;
            //        Travor_WorldState temp = new Travor_WorldState(this);
            //        temp.allAgentsState[agentTurn].move(6, time_G + 1);
            //        temp.allAgentsState[agentTurn].direction = 7;
            //        if (!Trevor_A_star.closedList.Contains(temp))
            //        {
            //            temp.setHuristic();
            //            temp.setTotalTime_G();
            //            Trevor_A_star.openList.Add(temp);
            //            Trevor_A_star.closedList.Add(temp);
            //            Trevor.generated++;
            //        }
            //    }
            //    if (isValidMove(pos_X, pos_Y, pos_X, pos_Y - 1, agentTurn))
            //    {
            //        moves[agentTurn][2] = pos_X - 1;
            //        moves[agentTurn][3] = pos_Y - 1;
            //        Travor_WorldState temp = new Travor_WorldState(this);
            //        temp.allAgentsState[agentTurn].move(7, time_G + 1);
            //        temp.allAgentsState[agentTurn].direction = 8;
            //        if (!Trevor_A_star.closedList.Contains(temp))
            //        {
            //            temp.setHuristic();
            //            temp.setTotalTime_G();
            //            Trevor_A_star.openList.Add(temp);
            //            Trevor_A_star.closedList.Add(temp);
            //            Trevor.generated++;
            //        }
            //    }
            //}
        }

        public override string ToString()
        {
            string ans = "step num: "+time_G+" huristic: "+huristic_H+" total time: "+totalTime_G+"\n";
            foreach (AgentState temp in allAgentsState)
            {
                ans +=" agent" + temp.agent.agentNum + ": (" + temp.pos_X + "," + temp.pos_Y + ")\n";
            }
            return ans;
        }
        public override int GetHashCode()
        {
            int ans = 0;
            int[] prime = {1,2,3,5,7,11,13,17,19,23,29,31,37,41,43,47,53,59,61,71,73,79};
            for (int i = 0; i < allAgentsState.Length; i++)
            {
                ans += allAgentsState[i].pos_X * prime[i % 22];
                ans += allAgentsState[i].pos_Y * prime[(i + 10) % 22];
                ans += agentTurn * prime[(i + 5) % 22];
            }
            return ans;
        }

        public override bool Equals(object obj)
        {
            Travor_WorldState that = (Travor_WorldState)obj;
            if (this.agentTurn != that.agentTurn) return false;
            if (this.totalTime_G > that.totalTime_G)return false;
            for (int i = 0; i < allAgentsState.Length; i++)
            {
                if (!this.allAgentsState[i].Equals(that.allAgentsState[i]))
                    return false;
            }
            return true;
        }
        public LinkedList<Point> getAllPoints()
        {
            LinkedList<Point> ans = new LinkedList<Point>();
            for (int i = 0; i < allAgentsState.Length; i++)
            {
                ans.AddFirst(new Point(allAgentsState[i].pos_X, allAgentsState[i].pos_Y,allAgentsState[i].direction));
            }
            return ans;
        }
    }
}
